/*
  Copyright (c) 2010 Les Newell. All rights reserved

  This program is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

//Any modules included in this file will be loaded

//WARNING - you must no load more than MAX_MODULES modules. If you do, the manager will crash on bootup
//Try to keep inputs (such as receiver, analog inputs etc) at the beginning of the file and
//outputs (such as servos) at the end of the file



#define ERROR_PORT Serial

#include "serial.h" //Note this should always be the first as it handles error messages

SerialComms g_TelemetryComms(Serial3,9600);
SerialComms g_UsbComms(Serial,115200);

#include "altestimate.h"

#if 0

/*Route input channels to various parameters for debugging purposes
/* Note this only works for SocketF/PlugF signals*/


/*the number of inputs. If you want one input to drive more than one
output then define the input once for every output.*/
#define MAX_TWEAKERS 1

//Input plugs. Normally the receiver aux channels
static const char* tweakInputNames[MAX_TWEAKERS]=
{
    "Receiver.Aux.1",
//    "Receiver.Aux.1",
};

//Output plugs. Normally some module's parameters
static const char* tweakOutputNames[MAX_TWEAKERS]=
{
    "m_altFilter",
//    "GyroControl.Roll.I",
};

//Offset the input values by this amount
static float tweakOffsets[MAX_TWEAKERS]=
{
    1.06,
//    1.06,
};

//Then apply this gain.
static float tweakGains[MAX_TWEAKERS]=
{
    0.1,
//    0.001,
};

#include "tweaker.h"
#endif


#define GPS_PORT Serial2
#define GPS_BAUD 38500
//Update rate in samples/second
#define GPS_RATE 10
//Any initialization strings that need to be sent. Note you need to supply your own
//line end characters (if needed)
static const char gpsInit[]="$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n";
#include "gpsNMEA.h"


#include "dcmahrs.h"


//Available pins in the Mega are: 10,11,12,13 and 62 - 69 (Analog 8 - 15)
//Note 13 is not recommended as it runs the LED and is an output on bootup

#define RECEIVER_CHANNELS 6
static const int receiverChannel[RECEIVER_CHANNELS] =
{
    10, //Elevator (pitch)
    11, //Aileron  (roll)
    67, //Rudder   (yaw)
    12, //Throttle
    68, //Aux.1 - also Switch.1
    69, //Aux.2 - also Switch.2
};

#include "pwmreceiver.h"

enum{FLIGHTMODE_DISARMED,FLIGHTMODE_ARMED,FLIGHTMODE_ACRO,FLIGHTMODE_HHOLD,MAX_FLIGHTMODES};
//Note this should be just after the receiver, before any splices.
#include "flightmode.h"


#include "positionVTOL.h"


//SPI
#define SPISLAVESELECT 53
#define SPICLOCK 52
#define SPIDATAOUT 51  //MOSI
#define SPIDATAIN 50    //MISO

#define SCPSELECT 49
#define SCPDRDY 48
#include "SCP1000.h"

//compensate for compass orientation
#define HMC5843_X AXISX
#define HMC5843_Y AXISY
#define HMC5843_Z AXISZ
//Use 1 or -1 to set the axis direction
#define HMC5843_X_DIRECTION -1
#define HMC5843_Y_DIRECTION 1
#define HMC5843_Z_DIRECTION -1
#include "hmc5843.h"

#define GYROADC_PITCH 2
#define GYROADC_ROLL 3
#define GYROADC_YAW 8
#define AZPIN 22
#define AZYAWPIN 23
//this is just a default starting point. Adjust in config to suit your setup
#define GYRO_SCALE 0.00055
#include "analoggyro.h"

#define ACCEL_ADC_X 7
#define ACCEL_ADC_Y 6
#define ACCEL_ADC_Z 5
//this is just a default starting point. Adjust in config to suit your setup
#define ACCEL_SCALE 0.001
#define ACCEL_OFFSET -550
#include "analogaccel.h"

//#include "accelahrs.h"

#include "rotarywing.h"



#define MAX_FLIGHTMODES 4


//the number of LED outputs
#define LED_OUTPUTS 5
//List the pin numbers here
static const byte ledPins[LED_OUTPUTS]=
{
    13,30,29,28,27,
};
//How long a sequence is
#define LED_SEQ_LENGTH 4
//1 is on, 0 is off
static const byte ledSequence[MAX_FLIGHTMODES][LED_SEQ_LENGTH][LED_OUTPUTS]=
{
    { //FLIGHTMODE_DISARMED
        {1,1,0,0,0},
        {0,0,0,0,0},
        {0,0,0,0,0},
        {0,0,0,0,0},
    },
    { //FLIGHTMODE_ARMED
        {1,1,1,0,0},
        {1,1,1,0,0},
        {0,0,0,1,1},
        {0,0,0,1,1},
    },
    { //FLIGHTMODE_ACRO
        {0,0,0,0,1},
        {1,0,0,1,0},
        {0,0,1,0,0},
        {1,1,0,0,0},
    },
    { //FLIGHTMODE_HHOLD
        {1,1,1,0,0},
        {1,0,0,0,0},
        {0,0,0,1,1},
        {1,0,0,0,0},
    },
};
#include "led.h"


//make this 1 if you want tricopter with yaw arm at the rear
#if 1
//Set how fast you want to update. Usually this would be
//the same as the FlightControl rate
#define MATRIX_INTERVAL 5
#define MATRIX_OUTPUTS 4
//Increases the power to the yaw motor when turning to compensate
//for the loss of lift
#define YAW_COMPENSATION 0.1
#define YAW_MOTOR 1

//Note the offset is 0 for a servo (zero in the middle) and -1
//for a motor (zero at min pulse width)
const float matrixGains[MATRIX_OUTPUTS][MAX_COMMANDS + 1]=
{
// pitch   roll    yaw  throttle  offset
    {0      ,0      ,-1     ,0      ,0   }, //Yaw servo
    {-0.25  ,0      ,0      ,0.9   ,-1  }, //Yaw motor
    {0.125  ,0.216  ,0      ,0.9   ,-1  }, //Right motor
    {0.125  ,-0.216 ,0      ,0.9   ,-1  }, //Left motor
};

//Outputs when the craft is disarmed. Remember -1 is min output.
const float motorDisarmed[MATRIX_OUTPUTS]=
{
    0,//Yaw servo
    -1,//yaw motor
    -1,//right motor
    -1,//left motor
};

const char matrixServo0[] PROGMEM = "Servo.1";
const char matrixServo1[] PROGMEM = "FastServo.1";
const char matrixServo2[] PROGMEM = "FastServo.2";
const char matrixServo3[] PROGMEM = "FastServo.3";

const char * matrixServos[MATRIX_OUTPUTS]={
    matrixServo0,
    matrixServo1,
    matrixServo2,
    matrixServo3,
};

#endif

#include "matrixkinematics.h"


//comment out the defines if you don't need one of the channels
#define CAMERA_PITCHSERVO "servo.2"
//#define CAMERA_ROLLSERVO "servo.3"
#include "camera.h"

//define how many servos to define
//The maximum is currently set to 12
#define SERVO50HZCHANNELS 2
//List your servo output pins. We can use any pins
static const char servo50HzPins[SERVO50HZCHANNELS]={32,34};
#include "servo50hz.h"

//define how many high speed servos/ESCs to define
//The maximum is currently set to 6

//We can only use pins 2,3,5,6,7,8 on atmega
#define FASTSERVO_CHANNELS 3
//rate in Hz. Min 50Hz, max 400Hz
#define FASTSERVO_RATE 400
//List your servo output pins.
static const char fastServoPins[FASTSERVO_CHANNELS]={8,7,6};
#include "fastservo.h"

